"""
#===============================================================================
# License
#===============================================================================
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

#===============================================================================
Copyright (c) 2009 bryan p. conrad
bconrad2@gmail.com

Repository Path:    
$HeadURL$

Created on:           Oct 10, 2009
Last Revised on:      $Date$
Version:              $Revision$
ID:                   $Id$
Author:               $Author$ :: (bryan)

#===============================================================================


#===============================================================================
# Documentation
#===============================================================================

"""

#===============================================================================
# Imports
#===============================================================================
import time
import os
import JT
import JT.Kinematics
import vtk
import numpy as np
import pylab
import win32gui
import cPickle as pickle
from marker_data_read import marker_data_read

def main():
    drr = JT.DRR()
    #style = vtk.vtkInteractorStyleTrackballCamera()
    #drr.iren.SetInteractorStyle(style)
     
    drr.renWin.SetSize(512,512)

    #canvas.iren.setFixedSize(512,512)
    image_path = r'C:\Users\bryan\phd\data\Spine3\Spine3_1\Movies\crop_512/Spine3_1_949250001.tif'
    #volume_path = 'C:/Users/bryan/bryan-code/2D3D/vert1/CT/ucharCTv1r1.mhd'
    volume_path = r'C:\Users\bryan\phd\data\Spine3\CT\Spine3.mhd'
#    volume_path = r'C:\Users\bryan\phd\data\Spine3\CT\9498.mhd'
    #sta_path = 'C:/Users/bryan/bryan-code/2D3D/vert1/CT/ucharCTv1r1.sta'
    cal_path = r'C:\Users\bryan\phd\data\Spine3\Spine3_1\Movies\crop_512\Calibration_cal_m.cal'
    
    drr.SetVolumeFileName(volume_path)
    drr.LoadCalFile(cal_path, 'external')
    drr.volume.SetOpacityLevel(0.20)
    drr.volume.SetOpacityThreshold(150)

    center = np.asarray(drr.volume.GetCenter()) - \
                     np.asarray(drr.volume.GetPosition())
    drr.volume.SetOrigin(center)
    #drr.volume.SetPosition(center)

    marker0 = create_sphere((1,1,0))
    marker1 = create_sphere((1,0,0))
    marker2 = create_sphere((0,1,0))
    marker3 = create_sphere((0,0,1))
    marker4 = create_sphere((0,1,1))
    vol_pos = np.asarray(drr.volume.GetPosition())
    vol_origin = np.asarray(drr.volume.GetOrigin())
    pos1 = np.asarray((-38.686599999999999, -58.078800000000001, 15.1663))
    pos2 = np.asarray((-4.6259267248021096, -48.761739879460436, 3.375645199507729))
    pos3 = np.asarray((14.936329937356517, -20.125035898071026, -3.80687285314667))
    #marker0.SetPosition(drr.volume.GetOrigin())
    marker1.SetPosition(vol_pos+pos1)
    marker2.SetPosition(vol_pos+pos2)
    marker3.SetPosition(vol_pos+pos3)
    marker4.SetPosition(0,0,0)
    assembly = vtk.vtkAssembly()
    
    assembly.AddPart(marker1)
    assembly.AddPart(marker2)
    assembly.AddPart(marker3)
    assembly.AddPart(drr.volume)
    drr.ren.RemoveAllViewProps()
    drr.ren.AddActor(assembly)
    
    background = JT.utils.genericImageReader(image_path)
    drr.SetBackgroundImage(background)
    
    drr.ren.ResetCamera()
    drr.xrayCamera.SetClippingRange(1,3000)
    drr.iren.Initialize()
    drr.Render()
    drr.iren.Start()



def test():
    drr = JT.DRR()
    style = vtk.vtkInteractorStyleTrackballCamera()
    drr.iren.SetInteractorStyle(style)
     
    drr.renWin.SetSize(512,512)

    #canvas.iren.setFixedSize(512,512)
    image_path = r'C:\Users\bryan\phd\data\Spine3\Spine3_1\Movies\crop_512/Spine3_1_949250001.tif'
    #volume_path = 'C:/Users/bryan/bryan-code/2D3D/vert1/CT/ucharCTv1r1.mhd'
    volume_path = r'C:\Users\bryan\phd\data\Spine3\CT\Spine3.mhd'
#    volume_path = r'C:\Users\bryan\phd\data\Spine3\CT\9498.mhd'
    #sta_path = 'C:/Users/bryan/bryan-code/2D3D/vert1/CT/ucharCTv1r1.sta'
    cal_path = r'C:\Users\bryan\phd\data\Spine3\Spine3_1\Movies\crop_512\Calibration_cal_m.cal'
    
    drr.SetVolumeFileName(volume_path)
    drr.LoadCalFile(cal_path, 'external')
    drr.volume.SetOpacityLevel(0.20)
    drr.volume.SetOpacityThreshold(150)
    center = np.asarray(drr.volume.GetCenter()) - \
                     np.asarray(drr.volume.GetPosition())
    drr.volume.SetOrigin(center)
    drr.volume.SetPosition(center)
    drr.volume.SetPosition(-26.0977, -28.1069, -500.915)
    drr.volume.SetOrientation(37.0594, -86.7517, 85.5201)

    find_marker_pose = 0
    if find_marker_pose:
        marker1 = create_sphere()
        marker2 = create_sphere()
        marker2.SetPosition(18.43507,31.50649,-7.31067)
        marker3 = create_sphere()
        marker3.SetPosition( 12.41679,65.60118,-14.77148)
        assembly = vtk.vtkAssembly()
        assembly.AddPart(marker1)
        assembly.AddPart(marker2)
        assembly.AddPart(marker3)
        #assembly.SetPosition(drr.volume.GetPosition())
        assembly.SetPosition(4.24318, 16.1462, -400.654)
        assembly.SetOrientation((64.9798, -167.427, 175.697))
        drr.ren.AddActor(assembly)
        
    else:
        marker0 = create_sphere((1,1,0))
        marker1 = create_sphere((1,0,0))
        marker2 = create_sphere((0,1,0))
        marker3 = create_sphere((0,0,1))
        marker4 = create_sphere((0,1,1))
        vol_pos = np.asarray(drr.volume.GetPosition())
#        vol_pos = center
#        pos1 = np.asarray((-38.686599999999999, -58.078800000000001, 15.1663))
#        pos2 = np.asarray((-4.6259267248021096, -48.761739879460436, 3.375645199507729))
#        pos3 = np.asarray((14.936329937356517, -20.125035898071026, -3.80687285314667))
        pos1 = np.asarray((5.3410259056832947, 16.545032356616566, -450.40167731414505))
        pos2 = np.asarray((17.526266583831237, 0.55961472214085051, -419.0657084199031))
        pos3 = np.asarray((25.961595602147625, 8.7893263136741666, -385.66738005329864))
        marker0.SetPosition(drr.volume.GetOrigin())
        marker1.SetPosition(pos1)
        marker2.SetPosition(pos2)
        marker3.SetPosition(pos3)
        marker4.SetPosition(0,0,0)
        #drr.ren.AddActor(marker0)
#        drr.ren.AddActor(marker1)
#        drr.ren.AddActor(marker2)
#        drr.ren.AddActor(marker3)
        assembly = vtk.vtkAssembly()
        assembly.AddPart(drr.volume)
        assembly.AddPart(marker1)
        assembly.AddPart(marker2)
        assembly.AddPart(marker3)
        #assembly.AddPart(marker4)
        #assembly.AddPart(marker0)
        drr.ren.RemoveAllViewProps()
        drr.ren.AddActor(assembly)
        assembly.SetOrigin(drr.volume.GetPosition())
        #assembly.SetPosition(-23.0977, -17.1069, -418.915)
        
    background = JT.utils.genericImageReader(image_path)
    drr.SetBackgroundImage(background)
    
    #drr.ren.ResetCamera()
    drr.xrayCamera.SetClippingRange(1,3000)
    drr.iren.Initialize()
    drr.Render()
    drr.iren.Start()
    if find_marker_pose:
        wxyz = assembly.GetOrientationWXYZ()
        pos = assembly.GetPosition()
        T1 = vtk.vtkTransform()
        T1.SetMatrix(assembly.GetMatrix())
        #T1.Transrighte(pos)
        T1.Translate(marker1.GetPosition())
        T1.RotateWXYZ(wxyz[0],wxyz[1:4])
        
        T2 = vtk.vtkTransform()
        T2.SetMatrix(assembly.GetMatrix())
        #T2.Translate(pos)
        T2.Translate(marker2.GetPosition())
        T2.RotateWXYZ(wxyz[0],wxyz[1:4])
        #
        T3 = vtk.vtkTransform()
        T3.SetMatrix(assembly.GetMatrix())
        #T3.Translate(pos)
        T3.Translate(marker3.GetPosition())
        T3.RotateWXYZ(wxyz[0],wxyz[1:4])
        #
        
        #T2.Translate(marker2.GetPosition())
        #T3.Translate(marker3.GetPosition())
        print T1.GetPosition()
        print T2.GetPosition()
        print T3.GetPosition()

def create_sphere(size=5,color=(1,0,0)):
    # Add axes to renderer
    sphere = JT.vtk.vtkSphereSource()
    sphere.SetRadius(size)
    sphere.SetPhiResolution(10)
    mapper = JT.vtk.vtkPolyDataMapper()
    sphere_actor = JT.vtk.vtkActor()
    mapper.SetInput(sphere.GetOutput())
    sphere_actor.SetMapper(mapper)
    props = sphere_actor.GetProperty()
    props.SetColor(color)
    return sphere_actor

def make_marker_stl(trc_path, marker_list=[],frame_range=[0,1],show=False):
    """
    Create an stl file from a trc, placing a sphere at the position defined 
    by each marker in the file.  
    
        trc_path is a trc filepath, which will be used to create the model.  
        marker list is a list of integers representing the markers 
            that should appear in the model.  
        frame_range is a first and last frame that should be averaged together to 
            determine the marker positions.  
        If show is True, a renderwindow will be displayed showing the model.
    """
    
    #motion_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed'
    #trc_list=os.listdir(motion_path)

    print "Processing: %s" % os.path.basename(trc)
    [n_frames, data, marker_names, data_rate, headers] = marker_data_read(trc)
    # Make all markers relative to first marker
    if marker_list == []:
        marker_list = range(len(marker_names))
    append_filter = vtk.vtkAppendPolyData()
    
    for m in marker_list:
        position = data[frame_range[0]:frame_range[1],m,:].mean(0)
        transform = vtk.vtkTransform()
        tpdf = vtk.vtkTransformPolyDataFilter()
        tpdf.SetTransform(transform)
        transform.Translate(position)
        sphere = JT.vtk.vtkSphereSource()
        sphere.SetRadius(7.5)
        sphere.SetPhiResolution(20)
        tpdf.SetInput(sphere.GetOutput())

        append_filter.AddInput(tpdf.GetOutput())
    
        mapper = JT.vtk.vtkPolyDataMapper()
        cluster_actor = JT.vtk.vtkActor()
        mapper.SetInput(append_filter.GetOutput())
        cluster_actor.SetMapper(mapper)
        props = cluster_actor.GetProperty()
        props.SetColor((0,0,1))
        
        stl_writer = vtk.vtkSTLWriter()
        stl_writer.SetInput(append_filter.GetOutput())
        stl_writer.SetFileTypeToASCII()
        stl_writer.SetFileName(filename.replace('trc','stl'))
        stl_writer.Write()
    
    if show:
        renWin = vtk.vtkRenderWindow()
        ren = vtk.vtkRenderer()
        iren = vtk.vtkRenderWindowInteractor()
        iren.SetRenderWindow(renWin)
        renWin.AddRenderer(ren)
        renWin.SetSize(512,512)
        ren.AddActor(cluster_actor)
        renWin.Render()
        iren.Start()
    

def marker_transform_from_file():
    """
    This function will calculate the joint angles between two vertebrae
    from motion capture data.  
    Inputs:     anatomical landmarks in CT coordinates
                marker list of markers attached to each vertebrae
                pyTrack project which has registered pose of markers and 
                    both volumes
                motion analysis marker data (trc)
    """             
#===============================================================================
#    Data Setup
#===============================================================================   
    # Flouro data
    jt_project = r'C:\Users\bryan\phd\data\Spine2\Spine2_C6C7_markers.xml'
    
    # markers attached to vertebrae
    marker_list_v1 = [13,15,17] # Spine2-C6
    marker_list_v2 = [16,18,20] # Spine2-C7
    motion_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed'
    trc = r'Spine2_5.trc'
    filename = os.path.join(motion_path,trc)
    print "Processing: %s" % trc
    [n_frames, marker_data, marker_names, data_rate, headers] = marker_data_read(filename)
    
    T_m_anat_v1_path = jt_project.replace('.xml','_T_m_anat_v1.pickle')
    fid = open(T_m_anat_v1_path,'r')
    T_m_anat_v1 = pickle.load(fid)
    print "T_m_anat_v1 LOADED from: %s" % str(T_m_anat_v1_path)
    fid.close()
    
    T_m_anat_v2_path = jt_project.replace('.xml','_T_m_anat_v2.pickle')
    fid = open(T_m_anat_v2_path,'r')
    T_m_anat_v2 = pickle.load(fid)
    print "T_m_anat_v2 LOADED from: %s" % str(T_m_anat_v2_path)
    fid.close()
#===============================================================================
#    Analysis
#===============================================================================
    T_out = []
    for frame in range(n_frames):
        # Global mocap marker transform
        p1_v1 = marker_data[frame,marker_list_v1[0],:]
        p2_v1 = marker_data[frame,marker_list_v1[1],:]
        p3_v1 = marker_data[frame,marker_list_v1[2],:]
        # Transform from mocap to markers
        T_mocap_m_v1 = JT.Kinematics.build_local(p1_v1,p2_v1,p3_v1)
        
        p1_v2 = marker_data[frame,marker_list_v2[0],:]
        p2_v2 = marker_data[frame,marker_list_v2[1],:]
        p3_v2 = marker_data[frame,marker_list_v2[2],:]
        # Transform from mocap to markers
        T_mocap_m_v2 = JT.Kinematics.build_local(p1_v2,p2_v2,p3_v2)
        
        # Do the actual transform
        # T_mocap_anat = T_mocap_m * T_c_m-1 * T_fl_c-1 * T_fl_v0 * T_v_v01-1 * T_v01_anat
        T_mocap_anat_v1 = np.dot(T_mocap_m_v1, T_m_anat_v1)
        T_mocap_anat_v2 = np.dot(T_mocap_m_v2, T_m_anat_v2)
        
        T = np.dot(np.linalg.inv(T_mocap_anat_v1),T_mocap_anat_v2)
        joint_angles = JT.Kinematics.get_pose(T,'213')
        #print mocap_anat_pose
        T_out.append(joint_angles)
    
    T_out = np.asarray(T_out)
    fig = pylab.figure()
    for i in range(3):
        sp = fig.add_subplot(6,1,i+1)
        sp.plot(T_out[:,i])
    for i in range(3,6):
        sp = fig.add_subplot(6,1,i+1)
        sp.plot(T_out[:,i]*180/np.pi)
    pylab.show()
    

def marker_local_coords():
    """
    Once the marker cluster stl has been co-registered with the volume(s) in JT, 
    the transformation between the volume and each marker can be determined.
    """
    # Setup paths
    #pos = np.asarray([ 23.7819519, 120.47001648, -359.72583008])
    #ori = np.asarray([-1.50492156, 0.55036956, 0.53518271])*180/np.pi
    motion_path = r'C:\Users\bryan\phd\data\Spine Motion\Spine_072106\Processed'
    trc = r'Spine2_2.trc'
    jt_project = r'C:\Users\bryan\phd\data\Spine2\Spine2_C6C7_markers.xml'
    
    # markers attached to vertebrae
    marker_list_v1 = [13,15,17] # Spine2-C6
    marker_list_v2 = [16,18,20] # Spine2-C7
    
    # Load data from xml file
    data = JT.Data2.DataStructure()
    data.load(jt_project)
    poses = data.get_all_poses()
    print data.get_unique_path_list('Volume')
    
    # Get Poses from data structure
    vol_paths = data.get_unique_path_list('Volume')
    img_path = data.get_current_image_path()
    
    vol1_pose = data.get_all_poses(vol_paths[0])
    vol2_pose = data.get_all_poses(vol_paths[1])
    cluster_pose = data.get_all_poses(vol_paths[2])
      
    vol1_pose = vol1_pose[0,0,:]
    vol2_pose = vol2_pose[0,0,:]
    cluster_pose = cluster_pose[0,0,:]
    
    # find Transformations
    T_g_v01 = JT.Kinematics.get_transform(vol1_pose,seq='213')
    T_g_v02 = JT.Kinematics.get_transform(vol2_pose,seq='213')
    T_g_c = JT.Kinematics.get_transform(cluster_pose,seq='213')
    
    filename = os.path.join(motion_path,trc)
    print "Processing: %s" % trc
    [n_frames, marker_data, marker_names, data_rate, headers] = marker_data_read(filename)
    
    center1 = data.get_center(img_path, vol_paths[0])
    center2 = data.get_center(img_path, vol_paths[1])
    T_v1_v01 = JT.Kinematics.get_transform((center1[0],center1[1],center1[2],0,0,0),seq='213')
    T_v2_v02 = JT.Kinematics.get_transform((center2[0],center2[1],center2[2],0,0,0),seq='213')  
    
    T_marker_list = []
    T_v01_m_list = []
    T_v02_m_list = []
    # Make all markers relative to first marker
    key = 0
    first_marker = marker_data[0,key,:]
    print "Key Marker: %s" % marker_names[key]
    assembly3 = vtk.vtkAssembly()
    label_list = []
    #for m,marker in enumerate(marker_names):
    #marker_range = [12,13,14,15,16,17,18]
    marker_range = range(len(marker_names)-1)
    n_markers = len(marker_range)
    for m in marker_range:
        marker = marker_names[m]
        pos = marker_data[0,m,:]-first_marker
        #sphere3 = create_sphere(7.5,(i/(n_markers-1.0),1,0)) 
        #sphere3.SetPosition(pos)
        #assembly3.AddPart(sphere3)
        
        T_c_m = JT.Kinematics.get_transform((pos[0],pos[1],pos[2],0,0,0),seq='213')
        #print "T_c_m: %s" % str(T_c_m)
        T_marker_list.append(T_c_m)
     
        T_v01_m = np.dot(np.linalg.inv(T_g_v01),np.dot(T_g_c,T_c_m))
        T_v02_m = np.dot(np.linalg.inv(T_g_v02),np.dot(T_g_c,T_c_m))
        #T_v2_m = np.dot(np.linalg.inv(T_g_v2),np.dot(T_g_c,T))
        T_v01_m_list.append(T_v01_m)
        T_v02_m_list.append(T_v02_m)
        print "Marker #%i: %s -> %s" % (m,marker,str(T_v01_m[0:3,3]))
   
    # Cluster markers (wrt to the first marker)
    # Make all markers relative to first marker
    first_marker = marker_data[0,0,:]
    p_c1_v1 = marker_data[0,marker_list_v1[0],:]-first_marker
    p_c2_v1 = marker_data[0,marker_list_v1[1],:]-first_marker
    p_c3_v1 = marker_data[0,marker_list_v1[2],:]-first_marker
    
    p_c1_v2 = marker_data[0,marker_list_v2[0],:]-first_marker
    p_c2_v2 = marker_data[0,marker_list_v2[1],:]-first_marker
    p_c3_v2 = marker_data[0,marker_list_v2[2],:]-first_marker
    
    # Transform from cluster to markers
    T_c_m_v1 = JT.Kinematics.build_local(p_c1_v1,p_c2_v1,p_c3_v1)
    T_c_m_v2 = JT.Kinematics.build_local(p_c1_v2,p_c2_v2,p_c3_v2)
   
    # Test the results by rendering the same markers in the v1 frame and v2 frame
    assembly1 = vtk.vtkAssembly()
    assembly2 = vtk.vtkAssembly()
    
    #for m,marker in enumerate(marker_names):
    for i,m in enumerate(marker_range):
        marker = marker_names[m]
        #sphere1 = create_sphere(4.5,(i/(n_markers-1.0),.5,(n_markers-float(i))/n_markers))
        #sphere2 = create_sphere(4.5,(i/(n_markers-1.0),.5,(n_markers-float(i))/n_markers))
        sphere1 = create_sphere(4.5,(.4,.4,.4))
        sphere2 = create_sphere(4.5,(.4,.4,.4))
        pos1 = T_v01_m_list[i][0:3,3]
        pos2 = T_v02_m_list[i][0:3,3]
        sphere1.SetPosition(pos1)
        sphere2.SetPosition(pos2)
        assembly1.AddPart(sphere1)
        assembly2.AddPart(sphere2)
        
        label1 = vtk.vtkTextActor3D()
        label1.SetPosition(pos1[0]+2,pos1[1]+2,pos1[2]+2)
        props1 = label1.GetTextProperty()
        props1.SetFontSize(8)
        label1.SetOrientation(90,-180,180)
        label1.SetInput(str(m))
        #assembly1.AddPart(label1)
        
        label2 = vtk.vtkTextActor3D()
        label2.SetPosition(pos2[0]+2,pos2[1]+2,pos2[2]+2)
        props2 = label2.GetTextProperty()
        props2.SetFontSize(8)
        label2.SetOrientation(-90,180,180)
        label2.SetInput(str(m))
        #assembly2.AddPart(label2)
    
    print "Volume1: %s" % str(vol1_pose)
    print "Volume2: %s" % str(vol2_pose)
    print "Cluster: %s" % str(cluster_pose)
    
    assembly1.SetOrientation(vol1_pose[3:6]*180/np.pi)
    assembly2.SetOrientation(vol2_pose[3:6]*180/np.pi)
    assembly3.SetOrientation(cluster_pose[3:6]*180/np.pi)
    
    assembly1.SetPosition(vol1_pose[0:3])
    assembly2.SetPosition(vol2_pose[0:3])
    assembly3.SetPosition(cluster_pose[0:3])
    
    vol1 = JT.Volume2()
    vol1.SetVolumeFileName(r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C6_uchar_crop.mhd')
    center = np.asarray(vol1.GetCenter()) -\
                     np.asarray(vol1.GetPosition())
    vol1.SetOrigin(center)
    vol1.SetPose(vol1_pose)
    vol1.SetOpacityThreshold(80)
    axes1 = JT.AxesActor(5)
    
    vol2 = JT.Volume2()
    vol2.SetVolumeFileName(r'C:\Users\bryan\phd\data\Spine2\CT\Spine2_C7_uchar_crop.mhd')
    center = np.asarray(vol2.GetCenter()) -\
                     np.asarray(vol2.GetPosition())
    vol2.SetOrigin(center)
    vol2.SetPose(vol2_pose)
    vol2.SetOpacityThreshold(80)
    axes2 = JT.AxesActor(5)

    # Anatomic Landmarks in CT coordinates
    p1 = np.asarray((32,-60,-25.5))
    p2 = np.asarray((22,-35,-26))
    p3 = np.asarray((48,-41,-25))
    offset = np.asarray((10,-5,20))
    #offset = np.asarray((0,0,0))
    # Test marker coords from ImageJ (tip of SP)
    ant_pos = p1-offset
    left_pos = p2-offset
    right_pos = p3-offset
    
    T_v1_anat = JT.Kinematics.build_local(left_pos,right_pos,ant_pos)
    T_v2_anat = JT.Kinematics.build_local(left_pos,right_pos,ant_pos)
    
    line1 = vtk.vtkLineSource()
    line1.SetPoint1(p1-offset)
    line1.SetPoint2(p2-offset)
    line2 = vtk.vtkLineSource()
    line2.SetPoint1(p2-offset)
    line2.SetPoint2(p3-offset)
    line3 = vtk.vtkLineSource()
    line3.SetPoint1(p3-offset)
    line3.SetPoint2(p1-offset)
    
    ap = vtk.vtkAppendPolyData()
    ap.AddInput(line1.GetOutput())
    ap.AddInput(line2.GetOutput())
    ap.AddInput(line3.GetOutput())
    line_mapper = vtk.vtkPolyDataMapper()
    line_mapper.SetInput(ap.GetOutput())
    line_actor = vtk.vtkActor()
    line_actor.SetMapper(line_mapper)
    
    ant = create_sphere(2.5,(0,0,1))
    left = create_sphere(2.5,(0,1,0))
    right = create_sphere(2.5,(1,0,0))
    
    o = create_sphere(3,(1,1,1))
    axes1.SetPosition(-offset)
    axes2.SetPosition(-offset)
    o.SetPosition(-offset)
    ant.SetPosition(ant_pos)
    left.SetPosition(left_pos)
    right.SetPosition(right_pos)
    
    assembly1.AddPart(axes1)
    assembly1.AddPart(ant)
    assembly1.AddPart(left)
    assembly1.AddPart(right)
    assembly1.AddPart(line_actor)
    
    assembly2.AddPart(axes2)
    assembly2.AddPart(ant)
    assembly2.AddPart(left)
    assembly2.AddPart(right)
    assembly2.AddPart(line_actor)

    # Visualization Stuff
    renWin = vtk.vtkRenderWindow()
    ren = vtk.vtkRenderer()
    ren.AddActor(assembly1)
    ren.AddActor(assembly2)
    #ren.AddActor(assembly3)
    ren.AddVolume(vol1)
    ren.AddVolume(vol2)
    iren = vtk.vtkRenderWindowInteractor()
    style = vtk.vtkInteractorStyleTrackballCamera()
    iren.SetRenderWindow(renWin)
    iren.SetInteractorStyle(style)
    renWin.AddRenderer(ren)
    renWin.SetSize(512,512)
    
    #renWin.Render()
    #iren.Start()
    
    # Save the transform to file
    T_m_anat_v1 = np.dot(np.linalg.inv(T_c_m_v1), 
                             np.dot(np.linalg.inv(T_g_c), 
                                    np.dot(T_g_v01, 
                                           np.dot(np.linalg.inv(T_v1_v01), 
                                                  T_v1_anat)))) 
    T_m_anat_v2 = np.dot(np.linalg.inv(T_c_m_v2), 
                             np.dot(np.linalg.inv(T_g_c), 
                                    np.dot(T_g_v02, 
                                           np.dot(np.linalg.inv(T_v2_v02), 
                                                  T_v2_anat))))
    
    
    T_m_anat_v1_path = jt_project.replace('.xml','_T_m_anat_v1.pickle')
    fid = open(T_m_anat_v1_path,'w')
    pickle.dump(T_m_anat_v1, fid)
    print "T_m_anat_v1 SAVED to: %s" % str(T_m_anat_v1_path)
    fid.close()
    
    T_m_anat_v2_path = jt_project.replace('.xml','_T_m_anat_v2.pickle')
    fid = open(T_m_anat_v2_path,'w')
    pickle.dump(T_m_anat_v2, fid)
    print "T_m_anat_v2 SAVED to: %s" % str(T_m_anat_v2_path)
    fid.close()


if __name__ == '__main__':
#    marker_local_coords()
#    make_marker_stl()
    marker_transform_from_file()